#ifndef IMU_PROCESSOR__IMU_PROCESSOR_HPP_
#define IMU_PROCESSOR__IMU_PROCESSOR_HPP_

#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <tf2_ros/static_transform_broadcaster.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>
#include <tf2/LinearMath/Vector3.h>
#include <std_srvs/srv/trigger.hpp>
#include <memory>
#include <array>
#include <Eigen/Dense>
#include <vector>
#include <numeric>

class SimpleKalmanFilter {

public:

    SimpleKalmanFilter(double process_noise, double measurement_noise);
    double filter(double measurement);
    void reset();
    bool is_initialized() const { return initialized_; };
    void update_process_noise(double new_process_noise);

private:

    //状态估计
    double x_;  //状态值
    double P_;  //估计误差协方差
    double Q_;  //过程噪声协方差
    double R_;  //测量噪声协方差
    bool initialized_;
};

class IMUCalibrator {
public:
    struct Bias {
        double gyro_x = 0.0, gyro_y = 0.0, gyro_z = 0.0;
        double accel_x = 0.0, accel_y = 0.0, accel_z = 0.0;
    };

private:
    std::vector<double> gyro_x_samples, gyro_y_samples, gyro_z_samples;
    std::vector<double> accel_x_samples, accel_y_samples, accel_z_samples;
    bool is_calibrating_ = false;
    size_t sample_count_ = 0;
    size_t max_samples_ = 1000;
    Bias bias_;

public:
    void start_calibration();
    void update_calibration(const sensor_msgs::msg::Imu::SharedPtr imu_msg);
    void compute_bias();
    sensor_msgs::msg::Imu apply_calibration(const sensor_msgs::msg::Imu& raw_imu) const;
    const Bias& get_bias() const { return bias_; }
    bool is_calibrating() const { return is_calibrating_; }
    size_t get_sample_count() const { return sample_count_; }
    size_t get_max_samples() const { return max_samples_; }
};


class ImuProcessor : public rclcpp::Node  {

public:

    ImuProcessor();

private:

    //ros2对象
    rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_sub_;
    rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_pub_;

    //服务
    rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr calibrate_service_;

    //tf广播器
    std::shared_ptr<tf2_ros::StaticTransformBroadcaster> static_broadcaster_;

    //IMU安装姿态校正
    tf2::Matrix3x3 imu_rotation_matrix_;
    tf2::Quaternion imu_mount_orientation_;
    


    //参数变量
    std::string input_imu_topic_;
    std::string output_imu_topic_;
    std::string imu_frame_;
    std::string base_frame_;
    double imu_position_x_, imu_position_y_, imu_position_z_;
    double imu_rotation_x_, imu_rotation_y_, imu_rotation_z_, imu_rotation_w_;
    bool publish_tf_;
    bool use_kalman_filter_;
    bool use_calibration_;
    //卡尔曼滤波参数
    double process_noise_;
    double gyro_noise_;
    double accel_noise_;
    double static_process_noise_;
    double dynamic_process_noise_;
  
    // 卡尔曼滤波器
    std::unique_ptr<SimpleKalmanFilter> angular_vel_x_filter_;
    std::unique_ptr<SimpleKalmanFilter> angular_vel_y_filter_;
    std::unique_ptr<SimpleKalmanFilter> angular_vel_z_filter_;
    std::unique_ptr<SimpleKalmanFilter> linear_acc_x_filter_;
    std::unique_ptr<SimpleKalmanFilter> linear_acc_y_filter_;
    std::unique_ptr<SimpleKalmanFilter> linear_acc_z_filter_;

    //校准器
    IMUCalibrator calibrator_;

    void initialize_rotation_matrix();
    sensor_msgs::msg::Imu apply_rotation_correction(const sensor_msgs::msg::Imu& imu_data) const;
    bool is_zero_covariance(const std::array<double, 9>& cov);
    void publish_static_tf();
    bool is_vehicle_stationary(const sensor_msgs::msg::Imu& imu_data);
     double get_angular_velocity_magnitude(const sensor_msgs::msg::Imu& imu_data);
    double get_linear_acceleration_magnitude(const sensor_msgs::msg::Imu& imu_data);

    void imu_callback(const sensor_msgs::msg::Imu::SharedPtr raw_imu);
    void calibrate_callback(
        const std::shared_ptr<std_srvs::srv::Trigger::Request> request,
        std::shared_ptr<std_srvs::srv::Trigger::Response> response);

};

#endif